// MyselfCalibration.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include <vector>
#include <iostream>
#include <fstream>
#include <io.h>
#include <string>
#include <cv.h>
using namespace std;
#pragma comment(lib,"opencv_core220d.lib")
#pragma comment(lib,"opencv_imgproc220d.lib")
#pragma comment(lib,"opencv_highgui220d.lib")
#pragma comment(lib,"opencv_calib3d220d.lib")
#pragma comment(lib,"opencv_legacy220d.lib")
#include "Matrix3d.h"
#include "Matrix.h"

ostream& operator<<(ostream& out,Matrix3dC& ff) 
{
	for (int i=0;i<3;i++)
	out<<ff[i][0]<<" "<<ff[i][1]<<" "<<ff[i][2]<<endl;
	return out;
}
#include "findRT.h"
ostream& operator<<(ostream& out,MatrixC& ff)
{
	for (int i=0;i<ff.matCV->rows;i++)
	{
		
		for (int j=0;j<ff.matCV->cols;j++)
		{
		//	float a=ff[0][1];
			out<<ff[i][j]<<" ";
		}
	   out<<endl;
	}
	return out;
}
Vector3dC operator*(double& aa,Vector3dC& bb)
{
	Vector3dC ret;
	for(int i=0;i<3;i++)
		ret.a[i]=bb.a[i]*aa;
	return ret;
}
// int _tmain(int argc, _TCHAR* argv[])
// {
// 	//MatrixC aa(5,5);
// //    aa.fill(6.0);
//   /* cout<<aa;*/
//  	Vector3dC aa(1,2,3);
// // 	Vector3dC bb(1,2,3);
// // 	Matrix3dC ff(1,1,1,1,1,1,1,1,1);
// // 	Vector3dC cc=ff*aa+bb;
// //	Vector3dC aa(1,1,1);
// // 	Vector3dC aa(1,2,3);
// // 	const Vector3dC bb(1,2,3);
// // 	Vector3dC cc=aa+bb;
// 	//int t=1;
// 	//aa[t][0]=10;
// 	aa[1]=2*aa[1];
// 	cout<<aa[1];
// 	return 0;
// }
int inputpoint(string datap,vector<CalObservation2dArrayC>& obs)
{

	int ncameras=0;
	char fnPoints[256];
	datap=datap+"points";
	do
	{
		sprintf(fnPoints, "%s.%02d.txt",datap.c_str() , ncameras++);
	}
	while (access(fnPoints, 04) == 0);
	--ncameras;

	if (ncameras == 0)
	{
		cerr << "ERROR: " << fnPoints << " not found" << endl;
		return 0;
	}
	cerr << "Loading data for " << ncameras << " cameras." << endl;
	obs.resize(ncameras);
	for (int icam = 0; icam < ncameras; icam++)
	{
		sprintf(fnPoints, "%s.%02d.txt", datap.c_str(), icam);
		ifstream filePoints(fnPoints);
		if (!filePoints.good())
		{
			cerr << "ERROR: couldn't open " << fnPoints << endl;
			return 0;
		}

		int rows = 0;
		filePoints >> rows;    
		obs[icam] = CalObservation2dArrayC(rows,CalObservation2dC());
		for (int r = 0; r < rows; r++)
		{
			float u, v;
			filePoints >> u >> v;
			obs[icam][r] = CalObservation2dC(u,v,((u > 0) && (v > 0)) ? true : false);
		}
		// all views must have same number
		if(obs[icam].size()!= obs[0].size())
			return 0;
	}

	return 1;
}
CvMat* GetFundamentalMat(vector<CalObservation2dArrayC>& obs,int icam,int jcam,CvMat** fundamental_mat)
{
	int point_count=obs[icam].size();
   CvMat* point1=cvCreateMat(1,point_count,CV_32FC2);
   CvMat* point2=cvCreateMat(1,point_count,CV_32FC2);
   CvMat* status=cvCreateMat(1,point_count,CV_8UC1);
   CvMat* fundamental_matrix=cvCreateMat(3,3,CV_32FC1);
   *fundamental_mat=fundamental_matrix;
   cvSetZero(point1);cvSetZero(point2);
   int ivisible=0;
   for (int i=0;i<point_count;i++)
   {    
	   if (obs[icam][i].visible&&obs[jcam][i].visible)
      {
		  point1->data.fl[ivisible*2]  =obs[icam][i].u[0];
		  point1->data.fl[ivisible*2+1]=obs[icam][i].u[1];
		  point2->data.fl[ivisible*2]  =obs[jcam][i].u[0];
		  point2->data.fl[ivisible*2+1]=obs[jcam][i].u[1];
		  ivisible++;
      }
   }
   cvSave("C:\\Documents and Settings\\User\\My Documents\\Visual Studio 2008\\Projects\\MyCalibration\\MyCalibration\\data\\test\\point0.xml",point1);
   cvSave("C:\\Documents and Settings\\User\\My Documents\\Visual Studio 2008\\Projects\\MyCalibration\\MyCalibration\\data\\test\\point1.xml",point2);
   if(point1->cols>8)
	cvFindFundamentalMat(point1,point2,fundamental_matrix,CV_FM_RANSAC,1.0,0.99,status);
   if(point1->cols==8)
	  cvFindFundamentalMat(point1,point2,fundamental_matrix,CV_FM_8POINT,1.0,0.99,status);
	return status;
}
// bool GetFundamentalMat(CvMat* fundamental_mat,CalObservation2dArrayC& obsi,CalObservation2dArrayC& obsj)
// {
// 	int point_count=obs[icam].size();
// 	CvMat* point1=cvCreateMat(1,point_count,CV_32FC2);
// 	CvMat* point2=cvCreateMat(1,point_count,CV_32FC2);
// 	CvMat* status=cvCreateMat(1,point_count,CV_8UC1);
// 	cvSetZero(point1);cvSetZero(point2);
// 	int ivisible=0;
// 	for (int i=0;i<point_count;i++)
// 	{    
// 		if (obs[icam][i].visible&&obs[jcam][i].visible)
// 		{
// 			point1->data.fl[ivisible*2]  =obs[icam][i].u[0];
// 			point1->data.fl[ivisible*2+1]=obs[icam][i].u[1];
// 			point2->data.fl[ivisible*2]  =obs[jcam][i].u[0];
// 			point2->data.fl[ivisible*2+1]=obs[jcam][i].u[1];
// 			ivisible++;
// 		}
// 	}
// // 	cvSave("C:\\Documents and Settings\\User\\My Documents\\Visual Studio 2008\\Projects\\MyCalibration\\MyCalibration\\data\\test\\point0.xml",point1);
// // 	cvSave("C:\\Documents and Settings\\User\\My Documents\\Visual Studio 2008\\Projects\\MyCalibration\\MyCalibration\\data\\test\\point1.xml",point2);
// 	if(point1->cols>8)
// 		cvFindFundamentalMat(point1,point2,fundamental_matrix,CV_FM_RANSAC,1.0,0.99,status);
// 	if(point1->cols==8)
// 		cvFindFundamentalMat(point1,point2,fundamental_matrix,CV_FM_8POINT,1.0,0.99,status);
// 	cvReleaseMat(&point1);
// 	cvReleaseMat(&point2);
// 	cvReleaseMat(&status);
// 
// 	if(point1->cols<8)
// 		return false;
// 	return true;
// }
void inputcamera(string camerapara,vector<Matrix3dC>& cameraV)
{
	ifstream fileIntrinsics(camerapara.c_str());
	int ncameras;
	fileIntrinsics>>ncameras;
	for (int icam = 0; icam < ncameras; icam++)
	{
		float camera[4];
		fileIntrinsics >> camera[0] >>camera[1]>> camera[2] >> camera[3];
		Matrix3dC temp(camera[0],0        ,camera[2], \
				           0        ,camera[1],camera[3],\
						   0        ,        0,        1);
	    cameraV.push_back(temp);
	}

}


int _tmain(int argc, _TCHAR* argv[])
{
typedef vector<CvMat*> Matpair;
	vector<CalObservation2dArrayC> obs;
 //   inputpoint("C:\\Documents and Settings\\User\\My Documents\\Visual Studio 2008\\Projects\\MyCalibration\\MyCalibration\\data\\test\\",obs);
	 inputpoint("G:\\luan\\luoxun\\test6data2\\matlab\\",obs);
	vector<Matpair> fundamentV;
	for (int i=0;i<obs.size()-1;i++)
	{
		CvMat* fundaptr=NULL;
		CvMat* status=GetFundamentalMat(obs,i,i+1,&fundaptr);
		Matpair temp;
		temp.push_back(fundaptr);temp.push_back(status);
		fundamentV.push_back(temp);
	}
	Matrix3dC E;
	Matrix3dC K0;
	Matrix3dC K1;
	Matrix3dC F(fundamentV[0][0]);
	cout<<F;
	Vector2dC C0(320,240);
	Vector2dC C1(320,240);
	cout<<C0[0]<<" "<<C0[1]<<endl;
	FundamentalMatrixToEssential(E,K0,K1,F,C0,C1,1,1);
	cout<<E;
 	Matrix3dC R;Vector3dC t;
 	EssentialMatrixDecompose(R,t,E,K0,K1,obs[0][0],obs[1][0]);
	//PointFindGeometric(x, cam, obs);
 	cout<<R;
	cout<<t[0]<<" "<<t[1]<<" "<<t[2];
// 	vector<Matrix3dC> cameras;
//     inputcamera("G:\\luan\\luoxun\\data2\\dataS\\intrinsics12.txt",cameras);
// 	Matrix3dC E;
// 	Matrix3dC K0;
// 	Matrix3dC K1;
// 	Matrix3dC F(fundamentV[0][0]);
// 	Vector2dC C0(cameras[0][0][2],cameras[0][1][2]);
// 	Vector2dC C1(cameras[1][0][2],cameras[1][1][2]);
// 	float fxy1=cameras[0][0][0]/cameras[0][1][1];
// 	float fxy2=cameras[1][0][0]/cameras[1][1][1];;
// 	FundamentalMatrixToEssential(E,K0,K1,F,C0,C1,fxy1,fxy2);
	return 0;
}





// void ee()
// {
// 	CvMat* S=cvCreateMat(3,3,CV_32FC1); 
// 	CvMat* U=cvCreateMat(3,3,CV_32FC1); 
// 	CvMat* V=cvCreateMat(3,3,CV_32FC1); 
// 	CvMat* W=cvCreateMat(3,3,CV_32FC1); 
// 	CvMat* WT=cvCreateMat(3,3,CV_32FC1); 
// 	CvMat* VT=cvCreateMat(3,3,CV_32FC1); 
// 	CvMat* UT=cvCreateMat(3,3,CV_32FC1); 
// 
// 	CvMat* temp=cvCreateMat(3,3,CV_32FC1); 
// 
// 	cvmSet(W,0,0,0); cvmSet(W,0,1,-1);  cvmSet(W,0,2,0); 
// 	cvmSet(W,1,0,1); cvmSet(W,1,1,0);   cvmSet(W,1,2,0); 
// 	cvmSet(W,2,0,0); cvmSet(W,2,1,0);   cvmSet(W,2,2,1); 
// 
// 	cvTranspose(W,WT); 
// 
// 	cvSVD(essential,S,U,V); 
// 	cvTranspose(V,VT); 
// 
// 	//----------Set Correct parameters in S---------------------// 
// 	cvmSet(S,0,0,1); 
// 	cvmSet(S,1,1,1); 
// 	cvmSet(S,2,2,0); 
// 	//-------------Find Rotation Matrix----------------------------// 
// 	cvmMul(U,W,temp); 
// 	cvmMul(temp,VT,rotation); 
// 
// 	cvTranspose(U,UT); 
// 
// 	cvMatMul(S,UT,translationX); 
// 	cvMatMul(W,translationX,translationX); 
// 	cvMatMul(U,translationX,translationX); 
// }